
//PS2  初始化
#include <PS2X_lib.h>  //for v1.6
/******************************************************************
   set pins connected to PS2 controller:
     - 1e column: original
     - 2e colmun: Stef?
   replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT        12  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        13  //17

/******************************************************************
   select modes of PS2 controller:
     - pressures = analog reading of push-butttons
     - rumble    = motor rumbling
   uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

// Reset func
void (* resetFunc) (void) = 0;
//PS2  初始化
#include <IRremote.h>                   // IRremote库声明 
#include <Servo.h>                       //调用“Servo”库，调制无刷电机PWM输出
Servo ESC;                               //无刷电机命名为“ESC”
String throttle;                         //定义接收字符串为“throttle”
const int buzzer = 2;                    // 定义喇叭引脚为2号数字引脚
const int RECV_PIN = 3;                  //定义红外接收器的引脚为3
const int ledPin =  4;                  // 定义灯引脚为4号数字引脚
const int far_RECV_PIN = 5;              //定义远红外避障器的引脚为5
const int light_PIN = 6;                  //定义光照检测器的引脚为6
int mTime;
bool heart = 0;
IRrecv irrecv(RECV_PIN);
decode_results results;   //解码结果放在 decode results结构的 result中

void setup()                             //初始化参数
{
  ESC.attach(9);                        // 定义无刷电机引脚为9号数字引脚，可以输出PWM
  Serial.begin(9600);                  //设置串口通讯的波特率为9600

  ESC.writeMicroseconds(1000);            //无刷电机初始为低电平
  pinMode(ledPin, OUTPUT);             //前灯继电器输出
  pinMode(buzzer, OUTPUT);             //喇叭输出
  pinMode(RECV_PIN, INPUT);           //红外接收器输入
  pinMode(far_RECV_PIN, INPUT);           //远红外避障器输入
  pinMode(light_PIN, INPUT);             //光照检测器输入
  irrecv.enableIRIn(); // 启动接收器
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  if (error == 0)
  {
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if (error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if (error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");



}

void Action(String throttle)//主调函数
{
  if (throttle == "stop")                  //暂停，速度减为零
  {
    Serial.println(" Stop");
    ESC.writeMicroseconds(1000);
    digitalWrite(buzzer, LOW);
    digitalWrite(ledPin, LOW);
    throttle = "0";
  }

  else if (throttle == "one")              //调节滑板速度为1
  {
    int i = ESC.readMicroseconds();
    Serial.print(i);
    Serial.print("--");
    if (i <= 1350)
      for (i; i <= 1350; i++)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    else
      for (i; i >= 1350; i--)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    Serial.println(i);
    Serial.println(" speed 1");
    for (int i = 0; i <= 0; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
    }
    throttle = "0";
  }

  else if (throttle == "two")              //调节滑板速度为2
  {
    int i = ESC.readMicroseconds();
    Serial.print(i);
    Serial.print("--");
    if (i <= 1550)
      for (i; i <= 1550; i++)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    else
      for (i; i >= 1550; i--)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    Serial.println(i);
    Serial.println(" speed 2");
    for (int i = 0; i <= 1; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "three")              //调节滑板速度为3
  {
    int i = ESC.readMicroseconds();
    Serial.print(i);
    Serial.print("--");
    if (i <= 1750)
      for (i; i <= 1750; i++)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    else
      for (i; i >= 1750; i--)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    Serial.println(i);
    Serial.println(" speed 3");
    for (int i = 0; i <= 2; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "four")              //调节滑板速度为4
  {
    int i = ESC.readMicroseconds();
    Serial.print(i);
    Serial.print("--");
    if (i <= 1950)
      for (i; i <= 1950; i++)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    else
      for (i; i >= 1950; i--)
      {
        ESC.writeMicroseconds(i);
        delay(3);
      }
    Serial.println(i);
    Serial.println(" speed 4");
    for (int i = 0; i <= 3; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "turn on")              //点亮前灯
  {
    digitalWrite(ledPin, HIGH);
    Serial.println(" turn on");
    digitalWrite(buzzer, HIGH);
    delay(500);
    digitalWrite(buzzer, LOW);
    throttle = "0";
  }

  else if (throttle == "turn off")              //熄灭前灯
  {
    digitalWrite(ledPin, LOW);
    Serial.println(" turn off");
    digitalWrite(buzzer, HIGH);
    delay(400);
    digitalWrite(buzzer, LOW);
    throttle = "0";
  }

  else if (throttle == "buzzer on")              //铃铛响
  {
    int times = 4;
    Serial.println(" buzzer on");
    for (int i = 0; i <= times; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "buzzer off")              //铃铛尖悦响
  {
    int times = 2;
    Serial.println(" buzzer off");
    for (int i = 0; i <= times; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(100);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }
}

void loop()                                       //主循环
{
  //  ESC.writeMicroseconds(1000);
  //  digitalWrite(buzzer, LOW);
  //  digitalWrite(ledPin, LOW);

  if (Serial.available() > 0)                   //手机蓝牙遥控
  {
    throttle  = Serial.readString();            //单片机接收数据赋值给字符串“throttle”
    Serial.println(" 手机蓝牙模式");
    if (throttle == "start")
    {
      Serial.println(" Start,  ready go!");     //开始启动程序
      while (1)
      {
        throttle  = Serial.readString();
        if (throttle.equals(""))
        {
          heart = 1;
          mTime = millis();
        }

        Action(throttle);                         //发送调用函数自变量
        if (throttle == "reset")              //重新启动
        {
          Serial.println(" Reset");
          digitalWrite(buzzer, LOW);
          digitalWrite(ledPin, LOW);
          ESC.writeMicroseconds(1000);
          break;
        }
        if (heart == 1)                      //判断是否蓝牙断开，进而自保护
        {
          int test = millis();
          if (test - mTime > 10000)
          {
            ESC.writeMicroseconds(1000);
            digitalWrite(buzzer, LOW);
            digitalWrite(ledPin, LOW);
            Serial.println(" Reset");
            heart = 0;

          }
        }
      }
    }
    delay(3000);
    Serial.println(" Waiting...");
  }

  if (irrecv.decode(&results))                     //开始接受IR数据
  {
    if (results.value == 0xFF38C7)//开始
    {
      Serial.println(" IR模式");
      Serial.println(results.value, HEX);//以16进制换行输出接收代码
      digitalWrite(buzzer, HIGH);
      delay(200);
      digitalWrite(buzzer, LOW);
      while (1)
      {
        if (irrecv.decode(&results))                     //开始接受IR数据
        {
          if (results.value == 0xFF906F)//9号重启
          {
            ESC.writeMicroseconds(1000);
            digitalWrite(buzzer, HIGH);
            delay(200);
            digitalWrite(buzzer, LOW);
            digitalWrite(ledPin, LOW);
            Serial.println(" Reset");
            break;
          }
          if (results.value == 0xFF9867)//暂停
          {
            ESC.writeMicroseconds(1000);
            digitalWrite(buzzer, LOW);
            digitalWrite(ledPin, LOW);
            Serial.println(" Stop");
          }
          if (results.value == 0xFF10EF)//减少
          {
            int i = ESC.readMicroseconds();
            Serial.print(i);
            Serial.print("--");
            int t = i;
            if (i > 1100)
              for (i; i >= t - 100; i--)
              {
                ESC.writeMicroseconds(i);
                delay(2);
              }
            digitalWrite(buzzer, HIGH);
            delay(200);
            digitalWrite(buzzer, LOW);
            Serial.println(i);
          }
          if (results.value == 0xFF5AA5)//增加
          {
            int i = ESC.readMicroseconds();
            Serial.print(i);
            Serial.print("--");
            int t = i;
            if (i < 1900)
              for (i; i <= t + 100; i++)
              {
                ESC.writeMicroseconds(i);
                delay(2);
              }
            digitalWrite(buzzer, HIGH);
            delay(400);
            digitalWrite(buzzer, LOW);
            Serial.println(i);
          }
          if (results.value == 0xFFA25D)//速度一
          {
            throttle = "one";
            Action(throttle);                         //发送调用函数自变量
          }
          if (results.value == 0xFF629D)//速度二
          {
            throttle = "two";
            Action(throttle);                         //发送调用函数自变量
          }
          if (results.value == 0xFFE21D)//速度三
          {
            throttle = "three";
            Action(throttle);                         //发送调用函数自变量
          }
          if (results.value == 0xFF22DD)//速度四
          {
            throttle = "four";
            Action(throttle);                         //发送调用函数自变量
          }
          if (results.value == 0xFF6897)//开灯
          {
            throttle = "turn on";
            Action(throttle);                         //发送调用函数自变量
          }
          if (results.value == 0xFFB04F)//关灯
          {
            throttle = "turn off";
            Action(throttle);
          }
          if (results.value == 0xFF18E7)//铃铛尖悦响
          {
            throttle = "buzzer off";
            Action(throttle);
          }
          if (results.value == 0xFF4AB5)//铃铛响
          {
            throttle = "buzzer on";
            Action(throttle);
          }

          irrecv.resume(); // 接收下一个值
        }
        else continue;
        delay(100);
      }

    }
    delay(50);
  }
  //  if (error == 1)
  //  { //skip loop if no controller found
  //    resetFunc();
  //  }

  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

    if (ps2x.ButtonPressed(PSB_START))        //will be TRUE as long as button is pressed
      Serial.println("Start is being held");
    if (ps2x.ButtonPressed(PSB_SELECT))
    {
      Serial.println("Reset");
      digitalWrite(buzzer, LOW);
      digitalWrite(ledPin, LOW);
      ESC.writeMicroseconds(1000);
    }

    if (ps2x.ButtonPressed(PSB_L3))
    {
      digitalWrite(ledPin, HIGH);
      Serial.println(" turn on");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_R3))
    {
      digitalWrite(ledPin, LOW);
      Serial.println(" turn off");
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_L2))
    {
      int times = 1;
      Serial.println(" buzzer on");

      digitalWrite(buzzer, HIGH);
      delay(1500);
      digitalWrite(buzzer, LOW);

      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_R2))
    {
      int times = 1;
      Serial.println(" buzzer off");
      for (int i = 0; i <= times; i++)
      {
        digitalWrite(buzzer, HIGH);
        delay(100);
        digitalWrite(buzzer, LOW);
        delay(400);
      }
      throttle = "0";
    }


    if (ps2x.ButtonPressed(PSB_CIRCLE))              //will be TRUE if button was JUST pressed
    {
      int i = ESC.read();
      if (i <= 80)
        for (i; i <= 80; i = i + 1)
        {
          ESC.write(i);
          delay(15);
        }
      else
        for (i; i >= 80; i = i - 1)
        {
          ESC.write(i);
          delay(15);
        }
      //ESC.writeMicroseconds(1350);
      Serial.println(" speed 1");
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_CROSS))              //will be TRUE if button was JUST pressed OR released
    {
      int i = ESC.read();
      if (i <= 100)
        for (i; i <= 100; i = i + 1)
        {
          ESC.write(i);
          delay(15);
        }
      else
        for (i; i >= 100; i = i - 1)
        {
          ESC.write(i);
          delay(15);
        }
      //ESC.writeMicroseconds(1550);
      Serial.println(" speed 2");
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_SQUARE))             //will be TRUE if button was JUST released
    {
      int i = ESC.read();
      if (i <= 125)
        for (i; i <= 125; i = i + 1)
        {
          ESC.write(i);
          delay(15);
        }
      else
        for (i; i >= 125; i = i - 1)
        {
          ESC.write(i);
          delay(15);
        }
      //ESC.writeMicroseconds(1650);
      Serial.println(" speed 3");
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_TRIANGLE))
    {
      int i = ESC.read();
      if (i <= 150)
        for (i; i <= 150; i = i + 1)
        {
          ESC.write(i);
          delay(15);
        }
      else
        for (i; i >= 150; i = i - 1)
        {
          ESC.write(i);
          delay(15);
        }
      //ESC.writeMicroseconds(1850);
      Serial.println(" speed 4");
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      throttle = "0";
    }
    if (ps2x.ButtonPressed(PSB_L1) || ps2x.ButtonPressed(PSB_R1))
    { //print stick values if either is TRUE
      Serial.println(" stop");
      ESC.writeMicroseconds(1000);
      digitalWrite(buzzer, HIGH);
      delay(1000);
      digitalWrite(buzzer, LOW);
      digitalWrite(ledPin, LOW);
    }
  }
  delay(50);
}
